#include "init.h"
#include "musica.h"
#include <p33FJ16GS504.h>

const unsigned char aerodynamic[] = {KEY_D5 | NOTE_16, KEY_FF4 | NOTE_16, KEY_B5  | NOTE_16, KEY_FF4 | NOTE_16,
                                     KEY_D5 | NOTE_16, KEY_FF4 | NOTE_16, KEY_B5  | NOTE_16, KEY_FF4 | NOTE_16,
                                     KEY_D5 | NOTE_16, KEY_FF4 | NOTE_16, KEY_B5  | NOTE_16, KEY_FF4 | NOTE_16,
                                     KEY_D5 | NOTE_16, KEY_FF4 | NOTE_16, KEY_B5  | NOTE_16, KEY_FF4 | NOTE_16,
                                     KEY_D5 | NOTE_16, KEY_GF4 | NOTE_16, KEY_B5  | NOTE_16, KEY_GF4 | NOTE_16,
                                     KEY_D5 | NOTE_16, KEY_GF4 | NOTE_16, KEY_B5  | NOTE_16, KEY_GF4 | NOTE_16,
                                     KEY_D5 | NOTE_16, KEY_GF4 | NOTE_16, KEY_B5  | NOTE_16, KEY_GF4 | NOTE_16,
                                     KEY_D5 | NOTE_16, KEY_GF4 | NOTE_16, KEY_B5  | NOTE_16, KEY_GF4 | NOTE_16,
                                     KEY_G5 | NOTE_16,  KEY_B4 | NOTE_16, KEY_E5  | NOTE_16, KEY_B4  | NOTE_16,
                                     KEY_G5 | NOTE_16,  KEY_B4 | NOTE_16, KEY_E5  | NOTE_16, KEY_B4  | NOTE_16,
                                     KEY_G5 | NOTE_16,  KEY_B4 | NOTE_16, KEY_E5  | NOTE_16, KEY_B4  | NOTE_16,
                                     KEY_G5 | NOTE_16,  KEY_B4 | NOTE_16, KEY_E5  | NOTE_16, KEY_B4  | NOTE_16,
                                     KEY_E5 | NOTE_16,  KEY_A5 | NOTE_16, KEY_CF5 | NOTE_16, KEY_A5  | NOTE_16,
                                     KEY_E5 | NOTE_16,  KEY_A5 | NOTE_16, KEY_CF5 | NOTE_16, KEY_A5  | NOTE_16,
                                     KEY_E5 | NOTE_16,  KEY_A5 | NOTE_16, KEY_CF5 | NOTE_16, KEY_A5  | NOTE_16,
                                     KEY_E5 | NOTE_16,  KEY_A5 | NOTE_16, KEY_CF5 | NOTE_16, KEY_A5  | NOTE_16, 0xFF
                                    };

int toneIndex = -1;
unsigned char toneDelay = 0;

void playSong()
{
    if (toneDelay == 0)
    {
        toneIndex++;
        unsigned char tone = aerodynamic[toneIndex];

        if (tone == 0xFF)
        {
            toneIndex = 0;
            tone = aerodynamic[0];
        }
        toneDelay = tone >> 6;
        motorbeep(tone & 0x3F);
    }
    else
    {
        toneDelay--;
    }
}

unsigned int orgPTPER;

void motorbeepstart()
{
    extern unsigned int PWM_CON_STATES[];
    IOCON1 = PWM_CON_STATES[2];
    IOCON2 = PWM_CON_STATES[4];
    IOCON3 = PWM_CON_STATES[2];
    orgPTPER = PTPER;
}

void motorbeep(unsigned char k)
{
    PTCON2 = 6 - (k / 12);
    k = k % 12;
    const unsigned int notes[] = {33488,31608,29834,28160,26579,25088,23680,22351,21096,19912,18795,17740};

    PTPER = notes[k];
    MDC = 200;
}

void motorbeepstop()
{
    extern unsigned int PWM_CON_STATES[];
    PTCON2 = 0;
    // all off
    IOCON1 = PWM_CON_STATES[0];
    IOCON2 = PWM_CON_STATES[0];
    IOCON3 = PWM_CON_STATES[0];
    PTPER = orgPTPER;
}

